#ifndef _MOTOR_H__
#define _MOTOR_H__
#include "os/types.h" // IWYU pragma: export
#include "controller/controller.h"
#include "utils/utils.h"
#include "bsp/bsp.h"

typedef struct {
	bool   			b_start;
	u8     			mode_running;
	float   		torque_command;
	float   		velocity_command;
	controller_t 	controller;
}motor_t;

void motor_init(motor_t *motor);
bool motor_start(motor_t *motor, u8 mode);
bool motor_stop(motor_t *motor);
void motor_adc_irq_handler(motor_t *motor);

extern motor_t g_motor[1];

__STATIC_INLINE motor_t *motor(int index) {
	return &g_motor[index];
}

__STATIC_INLINE controller_t *controller(motor_t *motor) {
	return &motor->controller;
}

__STATIC_INLINE void motor_set_target_velocity(motor_t *motor, float velocity) {
	motor->velocity_command = velocity;
}

__STATIC_INLINE void motor_set_target_torque(motor_t *motor, float torque) {
	motor->torque_command = torque;
}

__STATIC_INLINE float motor_get_encoder_velocity(motor_t *motor) {
	#if (CONFIG_USE_HALL_ENCODER)
		return hall_get_velocity(&motor->hall);
	#elif (CONFIG_USE_MAG_ENCODER)
		return encoder_get_velocity(&motor->encoder);
	#else
		return sim_encoder_get_velocity();
	#endif
}
	
__STATIC_INLINE float motor_get_encoder_angle(motor_t *motor) {
	#if (CONFIG_USE_HALL_ENCODER)
		return hall_get_elec_angle(&motor->hall);
	#elif (CONFIG_USE_MAG_ENCODER)
		return encoder_get_elec_angle(&motor->encoder);
	#else
		return sim_encoder_get_angle();
	#endif
}

#endif /* _MOTOR_H__ */
